/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/MethodSrc/pso.cpp
 * @Description  : 粒子群优化算法PSO的实现，包括粒子的初始化、更新、全局最优解的计算及优化过程。
 *                 提供了配置粒子、更新粒子位置和速度、计算最佳值及可行性等功能。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:40:13
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#include "pso.h"
#include "tools.h"

using namespace std;
/**
 * @brief 粒子群优化算法（Particle Swarm Optimization, PSO）的构造函数
 *
 * 初始化PSO算法所需的所有参数，并创建指定数量的粒子。
 *
 * @param num_particles 粒子数量
 * @param num_variables 每个粒子的变量数量
 * @param lower_bounds 每个变量的下界数组
 * @param upper_bounds 每个变量的上界数组
 * @param vmax 每个变量的最大速度
 * @param objective_function 目标函数指针，用于计算粒子的适应度
 * @param lower_weight 下界权重
 * @param upper_weight 上界权重
 * @param max_iterations 最大迭代次数
 * @param c1 粒子个体最优位置对自身速度的影响因子
 * @param c2 全局最优位置对粒子速度的影响因子
 * @param threshold 收敛阈值，当全局最优解的适应度变化小于此值时认为收敛
 * @param stopping_criteria 停止准则
 * @param pso_type PSO算法类型
 */
PSO::PSO(int num_particles, int num_variables, double* lower_bounds, double* upper_bounds,
	double* vmax, void (*objective_function)(Particle*), double lower_weight,
	double upper_weight, int max_iterations, double c1, double c2, double threshold,
	const std::string& stopping_criteria, const std::string& pso_type)
		: number_of_particles_(num_particles),
		max_iterations_(max_iterations),
		upper_weight_(upper_weight),
		lower_weight_(lower_weight),
		stopping_criteria_(stopping_criteria),
		pso_type_(pso_type),
		threshold_(threshold),
		weight_(0.7),
		error_(static_cast<int>(PSOErrorType::PSO_NO_ERROR)) 
{
	srand(static_cast<unsigned>(GetTickCount()));
	if (number_of_particles_ <= 0) {
		error_ = static_cast<int>(PSOErrorType::PSO_PARTICLE_INVALID);
		particles_ = nullptr;
	} else {
		particles_ = new Particle*[number_of_particles_];
		for (int i = 0; i < number_of_particles_; ++i) {
			particles_[i] = new Particle(num_variables, lower_bounds, upper_bounds, vmax, c1, c2, &weight_, objective_function);
		}
		best_particle_index_ = 0;
		UpdateGlobalBest();
	}
}


PSO::~PSO ()
{
	for (int i = 0; i < number_of_particles_; ++i) {
		delete particles_[i];
		particles_[i] = nullptr;
	}
	delete[] particles_;
	particles_ = nullptr;
}

/**
 * @brief 配置粒子位置和速度
 *
 * 根据给定的索引、位置和速度配置粒子。
 *
 * @param index 粒子的索引
 * @param pos 粒子的位置数组
 * @param vel 粒子的速度数组
 */
void PSO::ConfigureParticle (int index, const double* pos, const double* vel) 
{
	if (index >= 0 && index < number_of_particles_) {
		particles_[index]->ConfigureParticle(pos, vel);
	}
}

/**
 * @brief 更新所有粒子的位置和速度
 *
 * 遍历所有粒子，根据全局最优位置更新每个粒子的位置和速度。
 */
void PSO::UpdateParticles()
{
    for (int i = 0; i < number_of_particles_; ++i) {
		particles_[i]->UpdateParticle(particles_[best_particle_index_]->GetBestPosition());
  	}
}

/**
 * @brief 更新全局最优解
 *
 * 遍历所有粒子，通过比较当前粒子和当前最优粒子的可行性及目标函数值，更新全局最优粒子的索引。
 *
 * @details
 * 如果当前粒子或当前最优粒子为空，则跳过该粒子。
 * 比较当前粒子和当前最优粒子的可行性及目标函数值：
 * - 如果当前粒子可行而当前最优粒子不可行，则更新当前最优粒子索引为当前粒子索引。
 * - 如果当前粒子和当前最优粒子均可行，且当前粒子的目标函数值小于等于当前最优粒子的目标函数值，
 *   则更新当前最优粒子索引为当前粒子索引。
 *
 */
void PSO::UpdateGlobalBest() 
{
  	for (int i = 0; i < number_of_particles_; ++i) {
		if (!particles_[i] || !particles_[best_particle_index_]) {
			continue;
		}
		bool current_feasible = particles_[i]->GetFeasible();
		bool best_feasible = particles_[best_particle_index_]->GetBestFeasible();
		double current_value = particles_[i]->GetValue();
		double best_value = particles_[best_particle_index_]->GetBestValue();
		if (current_feasible || !best_feasible) {
			if (current_feasible != best_feasible || current_value <= best_value) {
				best_particle_index_ = i;
			}
		}
	}
}

/**
 * @brief 优化函数
 *
 * 使用粒子群优化算法进行优化。
 *
 * @return 迭代次数，如果发生错误则返回 -1 或 -2
 */
int PSO::Optimize() 
{
	int iterations = 0;
	if (GetError() != static_cast<int>(PSOErrorType::PSO_NO_ERROR)) {
		return -1;
	}
	while (iterations < max_iterations_) {
		if (stopping_criteria_ == "none" ||
			(stopping_criteria_ == "impBest" && ImpBest() >= threshold_) ||
			(stopping_criteria_ == "impAv" && ImpAv() >= threshold_) ||
			(stopping_criteria_ == "noAce" && NoAce() >= threshold_) ||
			(stopping_criteria_ == "movObj" && MovObj() >= threshold_) ||
			(stopping_criteria_ == "movPor" && MovPor() >= threshold_) ||
			(stopping_criteria_ == "stdDev" && StdDev() >= threshold_) ||
			(stopping_criteria_ == "maxDist" && MaxDist() >= threshold_) ||
			(stopping_criteria_ == "maxDistQuick" && MaxDistQuick() >= threshold_) ||
			(stopping_criteria_ == "comCrit" && ComCrit() >= threshold_) ||
			(stopping_criteria_ == "diffMaxDistQuick" && DiffMaxDistQuick() >= threshold_)) {
			weight_ = upper_weight_ - iterations * (upper_weight_ - lower_weight_) / max_iterations_;
			UpdateParticles();
			UpdateGlobalBest();
			++iterations;
		} else {
			error_ = static_cast<int>(PSOErrorType::PSO_PARTICLE_INVALID);
			return -2;
		}
	}
	return iterations;
}

/**
 * @brief 获取粒子群优化算法中的最佳值
 *
 * 该函数返回粒子群优化算法中的最佳值，即所有粒子中的最佳适应度值。
 *
 * @return double 最佳适应度值
 */
double PSO::GetBestValue() const 
{
	return particles_[best_particle_index_]->GetBestValue();
}

/**
 * @brief 获取粒子群优化算法中的最佳位置
 *
 * 返回当前粒子群优化算法中最佳粒子的最佳位置。
 *
 * @return 指向最佳位置的常量指针
 */
const double* PSO::GetBestPosition() const 
{
	return particles_[best_particle_index_]->GetBestPosition();
}

/**
 * @brief 获取当前粒子群算法中最优解的可行性
 *
 * 获取当前粒子群算法中最优解的可行性，即最优解是否可行。
 *
 * @return 如果最优解可行，则返回 true；否则返回 false。
 */
bool PSO::GetBestFeasibility() const {
	return particles_[best_particle_index_]->GetBestFeasible();
}

/**
 * @brief 打印当前最优粒子的最佳位置信息
 *
 * 此函数用于打印当前最优粒子的最佳位置信息。通过调用最优粒子的 PrintParticleBest 函数实现。
 */
void PSO::PrintBest() const 
{
	particles_[best_particle_index_]->PrintParticleBest();
}

/**
 * @brief 获取粒子群优化算法的错误码
 *
 * 如果算法本身存在错误，则返回算法的错误码；否则，遍历所有粒子，返回第一个粒子的错误码；
 * 如果所有粒子均无错误，则返回PSO_NO_ERROR。
 *
 * @return 返回粒子群优化算法的错误码，如果无错误则返回PSO_NO_ERROR。
 */
int PSO::GetError() const {
	if (error_ != static_cast<int>(PSOErrorType::PSO_NO_ERROR)) {
		return error_;
	}
	for (int i = 0; i < number_of_particles_; ++i) {
		int particle_error = particles_[i]->GetError();
		if (particle_error != PARTICLE_NO_ERROR) {
			return particle_error;
		}
	}
	return static_cast<int>(PSOErrorType::PSO_NO_ERROR);
}

/**
 * @brief 设置PSO类的错误码
 *
 * 将给定的错误码设置到PSO类的error_成员变量中。
 *
 * @param err 要设置的错误码
 */
void PSO::SetError(int err) 
{
  	error_ = err;
}

/**
 * @brief 返回当前粒子群优化算法中所有粒子的最佳值的绝对值
 *
 * 该函数返回粒子群优化算法中所有粒子的最佳值的绝对值。
 *
 * @return 当前粒子群优化算法中所有粒子的最佳值的绝对值
 */
double PSO::ImpBest() const
{
    return std::fabs(GetBestValue());
}

/**
 * @brief 计算并返回阈值加1的结果
 *
 * 该函数返回当前粒子群优化算法（PSO）对象的阈值加1的结果。
 *
 * @return 返回阈值加1的结果
 */
double PSO::ImpAv () const
{
	return threshold_ + 1;
}

/**
 * @brief 返回No Ace值
 *
 * 该函数返回No Ace值，即阈值加1。
 *
 * @return 返回No Ace值
 */
double PSO::NoAce () const
{
	return threshold_ + 1;
}

/**
 * @brief 移动目标函数
 *
 * 该函数用于计算粒子群优化算法中粒子的移动目标值。
 *
 * @return 返回粒子的移动目标值，即阈值加1。
 */
double PSO::MovObj () const
{
	return threshold_ + 1;
}

/**
 * @brief 移动粒子的位置
 *
 * 该函数返回当前粒子位置的阈值加1的值。
 *
 * @return 返回粒子的新位置，即当前位置阈值加1。
 */
double PSO::MovPor() const
{
	return threshold_ + 1;
}

/**
 * @brief 计算并返回标准差
 *
 * 返回标准差，该值等于阈值加1。
 *
 * @return double 标准差
 */
double PSO::StdDev() const
{
	return threshold_ + 1;
}

/**
 * @brief 获取最大距离
 *
 * 该函数返回PSO对象中的最大距离，即阈值加一。
 *
 * @return 返回最大距离值
 */
double PSO::MaxDist() const
{
	return threshold_ + 1;
}

/**
 * @brief 快速计算最大距离
 *
 * 该函数用于快速计算最大距离，返回值为阈值加一。
 *
 * @return 返回最大距离值，即阈值加一。
 */
double PSO::MaxDistQuick() const
{
	return threshold_ + 1;
}

/**
 * @brief 计算临界值
 *
 * 该函数返回粒子群优化算法中的临界值。临界值是通过将阈值加一得到的。
 *
 * @return 临界值，类型为 double
 */
double PSO::ComCrit () const
{
	return threshold_ + 1;
}

/**
 * @brief 快速计算粒子群优化算法中粒子间的最大距离差异
 *
 * 该函数用于计算粒子群优化算法中粒子间的最大距离差异。
 * 返回一个大于阈值的常数，以表示差异值。
 *
 * @return double 返回粒子间的最大距离差异值
 */
double PSO::DiffMaxDistQuick () const
{
	return threshold_ + 1;
}
